#include <system.h>


message_t xutil_msg;
BOOL xutil_busy = FALSE;


void XUTIL_pingSendDone(error_t err) {
	
	LEDS4_off(LEDS4_GREEN);
	xutil_busy = FALSE;
}

message_t* XUTIL_motorReceive(message_t* msg) {
	
	uint8_t* d = (uint8_t*) XPACKET_getData(msg);
	uint16_t lspeed, rspeed;
	
	if (d[6] != 0xaa) {
		LEDS4_toggle(LEDS4_RED);
		return msg;
	}
	
	LEDS4_toggle(LEDS4_OREN);
	
	if (d[0] == XMOTOR_FORWARD)
		XMOTOR_setDirection(XMOTOR_RIGHT, XMOTOR_FORWARD);
	else if (d[0] == XMOTOR_BACKWARD)
		XMOTOR_setDirection(XMOTOR_RIGHT, XMOTOR_BACKWARD);
	
	rspeed = d[1];
	rspeed <<= 8;
	rspeed |= d[2];
	
	XMOTOR_setSpeed(XMOTOR_RIGHT, rspeed);
	
	if (d[3] == XMOTOR_FORWARD)
		XMOTOR_setDirection(XMOTOR_LEFT, XMOTOR_FORWARD);
	else if (d[3] == XMOTOR_BACKWARD)
		XMOTOR_setDirection(XMOTOR_LEFT, XMOTOR_BACKWARD);
	
	lspeed = d[4];
	lspeed <<= 8;
	lspeed |= d[5];
	
	XMOTOR_setSpeed(XMOTOR_LEFT, lspeed);
	
	return msg;
}

message_t* XUTIL_pingReceive(message_t* msg) {
	
	if (xutil_busy) {
		
		return msg;
	}
	
	xutil_busy = TRUE;
	
	error_t err = XPACKET_send(XPACKET_TYPE_PING, &xutil_msg, 2);
	
	if (err != SUCCESS)
		xutil_busy = FALSE;
	else
		LEDS4_on(LEDS4_GREEN);
	
	return msg;
}